import numpy as np
import cv2  # 用于视频保存
import os

# 从大文件逐行读取并解析数据
def load_lidar_data_parse_rpy(file_path):
    data = []  # 用于存储所有帧数据
    current_frame = {}  # 当前帧数据
    points = []  # 点云数据

    try:
        with open(file_path, 'r', encoding='utf-8', errors='ignore') as f:  # 忽略无效字符
            for line in f:  # 使用生成器逐行读取文件
                line = line.strip()

                try:
                    if line.startswith("time_stamp:"):
                        # 如果当前帧已经有数据，保存到 data 中
                        if current_frame:
                            current_frame['points'] = np.array(points)  # 将点云数据转为 NumPy 数组
                            data.append(current_frame)
                            current_frame = {}
                            points = []

                        # 解析时间戳
                        current_frame['time_stamp'] = int(line.split(":")[1].strip())

                    elif line.startswith("fusepose:"):
                        # 解析 fusepose
                        current_frame['fusepose'] = list(map(float, line.split(":")[1].strip().split()))

                    elif line.startswith("rpy:"):
                        # 解析 rpy，只取前三个数
                        rpy_values = line.split(":")[1].strip().split()
                        current_frame['rpy'] = list(map(float, rpy_values[:3]))  # 只取前三个数

                    elif line.startswith("pcl_no:"):
                        # 解析点云数量（可选，不一定需要用到）
                        current_frame['pcl_no'] = int(line.split(":")[1].strip())

                    else:
                        # 解析点云数据
                        if line:  # 确保不是空行
                            points.append(list(map(float, line.split())))

                except ValueError as e:
                    # 忽略无法解析的行
                    print(f"解析行失败，跳过：{line}，错误：{e}")

        # 保存最后一帧
        if current_frame:
            current_frame['points'] = np.array(points)
            data.append(current_frame)

    except Exception as e:
        print(f"读取文件失败: {e}")

    return data


# 主程序
if __name__ == "__main__":
    # 文件路径
    # file_path = "/home/JSDC/017254/code/gitee/map_learing/map_grid/03_xyzrpy/vis_map/data1/save_lidar.csv"  # 替换为你的文件路径
    file_path = "/home/JSDC/017254/code/gitee/map_learing/map_grid/03_xyzrpy/rpy/data2/save_lidar.csv"  # 替换为你的文件路径

    # 加载数据
    lidar_data = load_lidar_data_parse_rpy(file_path)

    # 打印前 5 帧数据
    for i, frame in enumerate(lidar_data[:5]):  # 只打印前 5 帧
        print(f"Frame {i}:")
        print(f"  Time Stamp: {frame['time_stamp']}")
        print(f"  Fusepose: {frame['fusepose']}")
        print(f"  RPY: {frame['rpy']}")
        print(f"  Points: {frame['points'].shape} points loaded")
        print()

    # 计算 fusepose 的 x 和 y 的最小值和最大值
    fusepose_xy = np.array([frame['fusepose'][:2] for frame in lidar_data if 'fusepose' in frame])  # 提取所有 fusepose 的 x, y
    x_min, y_min = fusepose_xy[:, 0].min(), fusepose_xy[:, 1].min()  # 计算 x 和 y 的最小值
    x_max, y_max = fusepose_xy[:, 0].max(), fusepose_xy[:, 1].max()  # 计算 x 和 y 的最大值

    # 打印结果
    print(f"Fusepose x 范围: 最小值 = {x_min}, 最大值 = {x_max}")
    print(f"Fusepose y 范围: 最小值 = {y_min}, 最大值 = {y_max}")

    # 视频保存设置
    video_filename = "fusepose_mapping.avi"  # 视频文件名
    fps = 20  # 每秒帧数
    image_size = (1000, 1000)  # 图像大小 (高度, 宽度)
    fourcc = cv2.VideoWriter_fourcc(*'XVID')  # 视频编码格式
    video_writer = cv2.VideoWriter(video_filename, fourcc, fps, (image_size[1], image_size[0]))

    scale = 80.0  # 每 1.0 单位对应 100 个像素
    origin_pixel = (500, 900)  # 原点在图像中的位置 (x, y)

    # 创建空白图像用于保存轨迹
    trajectory_image = np.zeros(image_size, dtype=np.uint8)

    # 上一个点的像素坐标
    prev_pixel_x, prev_pixel_y = None, None

    # 创建空白图像
    for frame_idx, (x, y) in enumerate(fusepose_xy):
    # for frame_idx, (x, y) in enumerate(fusepose_xy[:1000]):
        # 当前帧的空白图像
        image = np.zeros(image_size, dtype=np.uint8)

        # 将实际坐标转换为像素坐标
        pixel_x = int(origin_pixel[0] + y * scale)  # x 负数往上
        pixel_y = int(origin_pixel[1] + x * scale)  # y 正数往右

        # 判断是否在图像范围内
        if 0 <= pixel_x < image_size[0] and 0 <= pixel_y < image_size[1]:
            # 绘制当前点
            cv2.circle(image, (pixel_x, pixel_y), 10, 255, -1)  # 注意 OpenCV 的坐标格式为 (列, 行)，即 (x, y)

            # 如果有上一个点，则绘制线段连接当前点和上一个点
            if prev_pixel_x is not None and prev_pixel_y is not None:
                cv2.line(trajectory_image, (prev_pixel_x, prev_pixel_y), (pixel_x, pixel_y), 255, 2)  # 线宽为 2

            # 更新上一个点的坐标
            prev_pixel_x, prev_pixel_y = pixel_x, pixel_y

        # 将轨迹图像叠加到当前帧图像上
        combined_image = cv2.add(image, trajectory_image)

        # 可选：绘制帧索引
        cv2.putText(combined_image, f"Frame: {frame_idx}", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255), 2)

        # 将图像转换为 RGB 格式（OpenCV 需要 3 通道图像）
        rgb_image = cv2.cvtColor(combined_image, cv2.COLOR_GRAY2BGR)

        # 保存帧为图片（可选）
        if frame_idx % 50 == 0:
            output_dir = '.'
            output_path = os.path.join(output_dir, f"results/frame_{frame_idx:04d}.png")  # 文件名格式为 frame_0001.png
            cv2.imwrite(output_path, rgb_image)

        # 写入视频
        video_writer.write(rgb_image)

        # 可选：显示图像（调试用）
        # cv2.imshow("Mapping", rgb_image)
        # if cv2.waitKey(1) & 0xFF == ord('q'):
        #     break

    # 释放视频写入器
    video_writer.release()
    print(f"视频已保存为 {video_filename}")
